#include "motor.h"


#include <stdbool.h>
#include "TM4C123GH6PM.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/pwm.h"

#include "FreeRTOS.h"
#include "task.h"

#include "myPWM/myPWM.h"
#include "myCAP/myCap.h"



// MOTOR 初始化, 一个是正反转，一个是PWM调速，周期为3200，初始占空比为0，频率为25Khz
uint16_t motor_PWMPeriod;
void Motor_Init(void)
{
  Motor_Dir_IO_Init(); // 方向控制IO初始化
  motor_PWMPeriod = 5000;
  M0PWM0_Init(SYSCTL_PWMDIV_64, motor_PWMPeriod); // 左电机控速PWM初始化，直接用系统时钟32分频，周期为1000，PWM频率为1.25Khz
  M0PWM1_Init(SYSCTL_PWMDIV_64, motor_PWMPeriod); // 右电机控速PWM初始化，直接用系统时钟32分频，周期为1000，PWM频率为1.25Khz

}

void Motor_Dir_IO_Init(void)
{
  // Enable the GPIO port
  SysCtlPeripheralEnable(MOTOR_L_IN1_GPIOPERIPH);
  SysCtlPeripheralEnable(MOTOR_L_IN2_GPIOPERIPH);
  SysCtlPeripheralEnable(MOTOR_R_IN1_GPIOPERIPH);
  SysCtlPeripheralEnable(MOTOR_R_IN2_GPIOPERIPH);
  // Enable the GPIO pins
  GPIOPinTypeGPIOOutput(MOTOR_L_IN2_GPIO, MOTOR_L_IN2_PIN);
  GPIOPinTypeGPIOOutput(MOTOR_L_IN1_GPIO, MOTOR_L_IN1_PIN);
  GPIOPinTypeGPIOOutput(MOTOR_R_IN1_GPIO, MOTOR_R_IN1_PIN);
  GPIOPinTypeGPIOOutput(MOTOR_R_IN2_GPIO, MOTOR_R_IN2_PIN);
  // 初始默认停车
  Motor_Stop();
}

void HallEncoder_Cap_Init()
{
  T1CCPBOTH_Init();
}

/**
 * @brief    指定速度直行
 * @param    pwmVal: 左右轮PWM占空比
 * @retval   void
 */
void Motor_Straight(uint16_t pwmVal)
{
  // 直行
  Motor_SetSpeed(pwmVal, pwmVal);
}

/**
 * @brief    指定速度倒车
 * @param    pwmVal: 左右轮PWM占空千分比的整数值[比如千分之500的占空比，那么就是传入500]
 * @retval   void
 */
void Motor_Back(uint16_t pwmVal)
{
  // 后退
  Motor_SetSpeed(-pwmVal, -pwmVal);
}

void MotorL_Stop(void)
{
  // AIN1=0,AIN2=0
  GPIOPinWrite(MOTOR_L_IN1_GPIO, MOTOR_L_IN1_PIN, 0);
  GPIOPinWrite(MOTOR_L_IN2_GPIO, MOTOR_L_IN2_PIN, 0);
}
void MotorR_Stop(void)
{
  // BIN1=0,BIN2=0
  GPIOPinWrite(MOTOR_R_IN1_GPIO, MOTOR_R_IN1_PIN, 0);
  GPIOPinWrite(MOTOR_R_IN2_GPIO, MOTOR_R_IN2_PIN, 0);
}
void Motor_Stop(void)
{
  // 停车
  MotorL_Stop();
  MotorR_Stop();
}

/**
 * @brief    控制左右轮旋转速度
 * @param    lPwmVal: 左轮PWM脉宽，负数表示反转
 * @param    rPwmVal: 右轮PWM脉宽，负数表示反转
 * @retval   void
 */
void Motor_SetSpeed(int16_t lPwmVal, int16_t rPwmVal)
{
  // left
  if (lPwmVal == 0)
  {
    MotorL_Stop();
  }
  else if (lPwmVal > 0)
  {
    MOTOR_L_DIR_Straight;
    PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, motor_PWMPeriod * 0.001 * lPwmVal);
  }
  else
  {
    MOTOR_L_DIR_Back;
    PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
    lPwmVal = -lPwmVal;
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, motor_PWMPeriod * 0.001 * lPwmVal);
  }

  // right
  if (rPwmVal == 0)
  {
    MotorR_Stop();
  }
  else if (rPwmVal > 0)
  {
    MOTOR_R_DIR_Straight;
    PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, motor_PWMPeriod * 0.001 * rPwmVal);
  }
  else
  {
    MOTOR_R_DIR_Back;
    PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);
    rPwmVal = -rPwmVal;
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, motor_PWMPeriod * 0.001 * rPwmVal);
  }
}
